/*************************************************
 *  Object:    Tricopter PID Control (Sensors)
 *  Date:      26/06/2011 (First)
 *             17/07/2011 (Last)
 *  Authors:   Teo, Carletto
 *************************************************/

/* Pinout */
#define LED            PORTC.B2   // LED
#define LED_DIR        TRISC.B2
#define M1             PORTB.B2   // Motor N.1
#define M1_DIR         TRISB.B2
#define M2             PORTB.B3   // Motor N.2
#define M2_DIR         TRISB.B3
#define M3             PORTB.B4   // Motor N.3
#define M3_DIR         TRISB.B4
#define MS             PORTB.B5   // Motor Servo
#define MS_DIR         TRISB.B5

/* Constants */
#define TABLE_DIM      16         // Table dimension for accelerometer data
#define MOTOR_ON       0          // 1 = Positive logic for motors
                                  // 0 = Negative logic for motors


unsigned short val_motors[4]; // 0 = M1 speed, 1 = M2 speed, 2 = M3 speed, 3 = MS speed
unsigned short acceleration[6];
unsigned short tdown;

char data_, error;  // DEBUG: some viariables to acquire motors speed from RS232


/* Smarty function to send on RS232 a char String
 */
void Soft_UART_Write_Text(char* testo) {
  char cnt;
  for (cnt=0; cnt<strlen(testo); cnt++) {
      Soft_UART_Write(testo[cnt]);
  }
}

/* Write a Byte of data to an address of ADXL345
 */
void ADXL345_Wr(unsigned short address, unsigned short in) {
    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(address);
    I2C1_Wr(in);
    I2C1_Stop();
}

/* Read a Byte of data from an address of ADXL345
 */
unsigned short ADXL345_Rd(unsigned short address) {
    unsigned short out;
    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(address);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(0x3B);      // Read Mode
    out = I2C1_Rd(0);
    I2C1_Stop();
    return out;
}

/* Fetch acceleration data from ADXL345
 *   NOTE: Address of 3 integer array
 *   must be provided!
 */
void ADXL345_getAcc(int *acc) {
    unsigned short cnt;
    unsigned short acc_tmp[6];
    char z_string[8];

    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(0x32);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(0x3B);      // Read Mode
    for (cnt=0; cnt<6; cnt++) {
        char ack;
        ack = (cnt<5);
        acc_tmp[cnt] = I2C1_Rd(ack);
    }
    I2C1_Stop();

    acc[0] = acc_tmp[1];
    acc[0] = acc[0] << 8;
    acc[0] |= acc_tmp[0];
    acc[1] = acc_tmp[3];
    acc[1] = acc[1] << 8;
    acc[1] |= acc_tmp[2];
    acc[2] = acc_tmp[5];
    acc[2] = acc[2] << 8;
    acc[2] |= acc_tmp[4];
}

/* Initial fetch of all values for the comparing table
 */
void dataArrayInit(int *fifox, int *fifoy, int *fifoz) {
     unsigned short cnt;
     int acc_tmp[3];
     for (cnt=0; cnt<TABLE_DIM; cnt++) {
         ADXL345_getAcc(&acc_tmp);
         fifox[cnt] = acc_tmp[0];
         fifoy[cnt] = acc_tmp[1];
         fifoz[cnt] = acc_tmp[2];
     }
}

/* Update the table with a newer value deleting the old one
 */
void dataArrayUpdate(int *fifox, int *fifoy, int *fifoz) {
     static unsigned short table_pointer;
     int acc_tmp[3];
     if (table_pointer < 0 || table_pointer >= TABLE_DIM) table_pointer = 0;
     ADXL345_getAcc(&acc_tmp);
     fifox[table_pointer] = acc_tmp[0];
     fifoy[table_pointer] = acc_tmp[1];
     fifoz[table_pointer] = acc_tmp[2];
     table_pointer++;
}

/* Filtering the axis value with comparing table
 */
int dataArrayFilter(int *fifo) {
     unsigned short cnt;
     long value = 0;
     for (cnt=0; cnt<TABLE_DIM; cnt++) {
         value += fifo[cnt];
     }
     return (int)(value/TABLE_DIM);
}

void Motor_ctrl(){
     unsigned short cnt, cnt1, max;
     cnt1 = max = 0;
     
     if(tdown==20) {
         M1 = M2 = M3 = MS = MOTOR_ON;
     }
     if(tdown==21) {
        tdown = 0;
        T0CON.TMR0ON=0;
        for(cnt=0; cnt<4; cnt++) {                              // Finds the maximum value of those set by PID
          max = (val_motors[cnt]>=max) ? val_motors[cnt] : max; // To not calculate every time the state of all lines
        }
        while(cnt1<=max) {
        if(cnt1==val_motors[0]) M1 = !MOTOR_ON;
        if(cnt1==val_motors[1]) M2 = !MOTOR_ON;
        if(cnt1==val_motors[2]) M3 = !MOTOR_ON;
        if(cnt1==val_motors[3]) MS = !MOTOR_ON;
        Delay_us(8);
        cnt1++;
        }
    cnt1=max=0; 
    TMR0L=70;
    T0CON.TMR0ON=1;
    }
}

/* ESC startup sequence
 * Sends to ESC throttle down for 4s
 */
void ESC_startup(){
     unsigned short cnt=0;
     while(cnt<200) {
         M1 = M2 = M3 = MOTOR_ON;
         Delay_ms(1);
         M1 = M2 = M3 = !MOTOR_ON;
         Delay_ms(20);
         cnt++;
     }
     T0CON.TMR0ON=1;
}

/* ESC configuration sequence
 * Sends to ESC throttle extremes
 * (all pulled down and all pulled up)
 */
void ESC_config(){
    unsigned short cnt=0;
    while(cnt<150){
        M1 = M2 = M3 = MOTOR_ON;
        Delay_ms(2);
        M1 = M2 = M3 = !MOTOR_ON;
        Delay_ms(20);
        cnt++;
    }
    cnt = 0;
    while(cnt<150){
        M1 = M2 = M3 = MOTOR_ON;
        Delay_ms(1);
        M1 = M2 = M3 = !MOTOR_ON;
        Delay_ms(20);
        cnt++;
    }
    Delay_ms(2000);
}

void main() {

    int acc_now[3];        // Accelerazione istantanea (0->X; 1->Y; 2->Z)
    int acc_x[TABLE_DIM];  // Array di accelerazioni asse X
    int acc_y[TABLE_DIM];  // Array di accelerazioni asse Y
    int acc_z[TABLE_DIM];  // Array di accelerazioni asse Z

    char x_string[8];
    char y_string[8];
    char z_string[8];

    /* Pinout direction assignment */
    LED_DIR = 0;      // LED is set as OUTPUT
    M1_DIR = 0;       // M1 is set as OUTPUT
    M2_DIR = 0;       // M2 is set as OUTPUT
    M3_DIR = 0;       // M3 is set as OUTPUT
    MS_DIR = 0;       // MS is set as OUTPUT
    
    /* Registers initialization */
    ADCON1 = 0x0F;    // Set A/D Disabled
    CMCON = 0xCF;     // Set Comparator Disabled

    /* Periphals initialization */
    Soft_UART_Init(&PORTB, 6, 7, 38400, 0);
    I2C1_Init(400000);
    
    Delay_ms(2000);
    
    /** Timer0 setup**/
     T0CON.T0PS0=1;     //Prescaler 1:1
     T0CON.T0PS1=0;
     T0CON.T0PS2=1;
     T0CON.PSA=0;       // Timer Clock Source is from Prescaler
     T0CON.T0CS=0;      // Prescaler gets clock from FCPU
     T0CON.T08BIT=1;    // 8 BIT MODE
     INTCON.TMR0IE=1;   // Enable TIMER0 Interrupt
     INTCON.PEIE=1;     // Enable Peripheral Interrupt
     INTCON.GIE=1;      // Enable global interrupt
     T0CON.TMR0ON=0;
     TMR0L=68;          // 1mS TMR0 preset
     TMR0H =0;
     
     /** Timer1 setup
    T1CON.T1CKPS1 =1;        //prescaler 1:8
    T1CON.T1CKPS0 =1;        //prescaler 1:8
    INTCON.GIE = 1 ;         //enable global interrupts
    INTCON.PEIE = 1 ;        //PEIE: Peripheral Interrupt Enable bit
    PIE1.TMR1IE = 1 ;        //enable timer 1 interrupt
    TMR1H = 0x00;            //High byte timer = 0
    TMR1L = 0x00;            //Low  byte timer = 0
    PIR1.TMR1IF = 0 ;        //clear timer 1 interrupt flag
    T1CON.TMR1ON = 0;        //timer 1 start/stop

    ADXL345_Wr(0x2C, 0x0B);      // 200Hz conversion data rate
    ADXL345_Wr(0x2D, 0x08);      // Measuring activated ;)
    ADXL345_Wr(0x31, 0x0B);      // Resolution set to +/- 16g (WOW!)
    Delay_ms(500);**/

    dataArrayInit(&acc_x, &acc_y, &acc_z);

    M1 = M2 = M3 = MS = !MOTOR_ON;
    tdown=0;
    val_motors[0]=0;
    val_motors[1]=0;
    val_motors[2]=0;
    val_motors[3]=0;
    //ESC_config();
    ESC_startup();
    val_motors[1]=10;
    val_motors[2]=30;
    val_motors[3]=60;
    

    while (1) {
           dataArrayUpdate(&acc_x, &acc_y, &acc_z);

           dataArrayFilter(&acc_x);
           dataArrayFilter(&acc_y);
           dataArrayFilter(&acc_z);
          /**
           Soft_UART_Write_Text("X: ");
           Soft_UART_Write_Text(&x_string);
           Soft_UART_Write_Text(" - Y: ");
           Soft_UART_Write_Text(&y_string);
           Soft_UART_Write_Text(" - Z: ");
           Soft_UART_Write_Text(&z_string);
           Soft_UART_Write(13);
           Soft_UART_Write(10);
             **/

             // wait until data is received
             do
                 data_ = Soft_UART_Read(&error);
             while (error);
             val_motors[0]=data_;

    }
}


void interrupt() {

     //INTCON.GIE = 0 ;    //disable interrupts

     if( INTCON.TMR0IF ){
          TMR0L=70;
          INTCON.TMR0IF=0;
          tdown++;
          Motor_ctrl();
     }


      //INTCON.GIE = 1 ;   //enable interrupts
 }


/**
Timer1: selezionando l'oscillatore interno abbiamo una f=fext/4->48M/4=12M
           Tclock=1/f  ->Ttimer=Tclock*(2^16- (T1H*2^8+T1L))

Timer0:   Tinterrupt = (256 - TMR0)*(4/fosc)*(Prescaler)
**/